
#include <util/mem.h>

#include "../../imp_types.h"

#include "../../matrix/matrix.h"
#include "../../dsp/filters/filters.h"

#include "../../geometry/geometry.h"
#include "../../geometry/delaunay.h"

#include "../image.h"
#include "../analysis/roi.h"
#include "../features/xcorner.h"
#include "../pointop/nonmaxsup.h"
#include "../pointop/threshold.h"

#include "../draw/draw.h"

#include "chessboard.h"

struct __triang_opt {
	boolean visited;
};

#define T_OPT(t) ((struct __triang_opt *)(IMP_OBJECT(t)->user_ptr))

struct __grid_point {
	boolean visited;
	int x;
	int y;
};

#define GRID(v) ((struct __grid_point *)(IMP_OBJECT(v)->user_ptr))

// <editor-fold defaultstate="collapsed" desc="Auxiliar Filt Chessboard">

/**
 * \brief Remove todos os pontos com menos de 2 vizinhos ou mais de 8 vizinhos
 * @param surf
 * @param changes
 */
static void __remove_invalid_points(ImpSurface * surf, int * changes) {

	ListIterator * it = list_get_first(surf->vertices);
	ImpVertex * v;

	(*changes) = 0;

	it = list_get_first(surf->vertices);
	while (it) {

		v = IMP_VERTEX(it->data);

		if (v->segments->size < 2 || v->segments->size > 8) {
			imp_surface_remove_vertex(surf, v);
			imp_object_delete(v);

			(*changes)++;
		}
		it = it->next;
	}
}

/**
 * \brief Removes all triangles by area.
 * @param surf
 * @param minArea
 * @param maxArea
 * @param changes
 */
static void __removes_triangles_by_area(ImpSurface * surf, real minArea, real maxArea, int * changes) {

	ListIterator * it = NULL;

	ImpTriangle * T;
	real a;

	(*changes) = 0;

	it = list_get_first(surf->faces);
	while (it) {

		T = IMP_TRIANGLE(it->data);
		it = it->next;

		a = imp_triangle_get_area(T);

		if (minArea > a || a > maxArea) {
			imp_surface_remove_triangle(surf, T);
			imp_object_delete(T);
			(*changes)++;
		}
	}
}

static boolean __triangle_has_color_transition(ImpTriangle * T, ImpMatrix * BW, real tx) {

	Ref2 p[3];
	Ref2 * ref;
	Ref2 bar;
	int idx;
	real val;
	boolean valid;

	// = Extract triangle points ===============================================
	ImpVertex * v1, * v2, * v3;
	imp_triangle_extract_vertices(T, &v1, &v2, &v3);

	p[0].x = IMP_POINT(v1)->x;
	p[0].y = IMP_POINT(v1)->y;

	p[1].x = IMP_POINT(v2)->x;
	p[1].y = IMP_POINT(v2)->y;

	p[2].x = IMP_POINT(v3)->x;
	p[2].y = IMP_POINT(v3)->y;

	// = Calculates internal triangle to avoid the borders =====================
	val = (p[0].x + p[1].x + p[2].x) / 3.0;
	bar.x = val;
	val = (p[0].y + p[1].y + p[2].y) / 3.0;
	bar.y = val;

	p[0].x = (bar.x - p[0].x) * tx + p[0].x;
	p[0].y = (bar.y - p[0].y) * tx + p[0].y;

	p[1].x = (bar.x - p[1].x) * tx + p[1].x;
	p[1].y = (bar.y - p[1].y) * tx + p[1].y;

	p[2].x = (bar.x - p[2].x) * tx + p[2].x;
	p[2].y = (bar.y - p[2].y) * tx + p[2].y;

	// = Get the points inside the internal triangle ===========================
	List * points = NULL;
	points = imp_image_roi_points(BW, p, 3);

	valid = FALSE; // returns TRUE by default

	ListIterator * it;
	it = list_get_first(points);

	ref = it->data;
	idx = imp_matrix_idx(BW, ref->x, ref->y);
	val = BW->data[idx];

	// = Verifies each point. If the value changes the current triangle is not valid.
	it = it->next;
	while (it) {

		ref = it->data;
		idx = imp_matrix_idx(BW, ref->x, ref->y);
		if (val != BW->data[idx]) {
			// = Invalid triangle ==============================================
			valid = TRUE;
			break;
		}
		it = it->next;
	}

	list_remove_all_data_full(points, free);

	return valid;
}

/**
 * \brief Removes all triangles with color transition inside.
 * @param surf
 * @param BW
 * @param tx
 * @param changes
 */
static void __removes_triangles_with_color_transition(ImpSurface * surf, ImpMatrix * BW, real tx, int * changes) {
	ListIterator * it = NULL;

	(*changes) = 0;

	it = list_get_first(surf->faces);
	while (it) {

		ImpTriangle * T;

		T = IMP_TRIANGLE(it->data);
		it = it->next;

		if (__triangle_has_color_transition(T, BW, tx)) {
			imp_surface_remove_triangle(surf, T);
			imp_object_delete(T);
			(*changes)++;
		}
	}
}

static int __count_triangle_neighbors_with_same_color(ImpTriangle * T, ImpMatrix * BW) {

	ImpTriangle * t = NULL;
	ImpPoint p;
	int i;

	real color_T, color_t;
	int nd;

	p = imp_triangle_barycenter(T);
	color_T = BW->data[ imp_matrix_idx(BW, p.x, p.y) ];


	nd = 0;
	for (i = 0; i < 3; i++) {
		t = imp_triangle_get_neighbor(T, T->e[i]);

		if (!t) continue;

		p = imp_triangle_barycenter(t);
		color_t = BW->data[ imp_matrix_idx(BW, p.x, p.y) ];

		if (color_T == color_t) nd++;
	}
	return nd;
}

/**
 * \brief Removes all triangles with the number of same color neighbors different of 1.
 * @param surf
 * @param BW
 * @param changes
 */
static void __removes_triangles_by_same_color_neighbors_count(ImpSurface * surf, ImpMatrix * BW, int * changes) {
	ListIterator * it = NULL;

	List * to_remove = list_new();

	(*changes) = 0;

	it = list_get_first(surf->faces);
	while (it) {

		ImpTriangle * T;
		int neT;

		T = IMP_TRIANGLE(it->data);
		it = it->next;

		// = Removes triangles with 0, 2 or more neighborhoods with the same color.
		neT = __count_triangle_neighbors_with_same_color(T, BW);
		if (neT != 1) {
			list_append(to_remove, T);
			(*changes)++;
			continue;
		}
	}

	it = list_get_first(to_remove);
	while (it) {

		ImpTriangle * T;
		T = IMP_TRIANGLE(it->data);
		it = it->next;

		imp_surface_remove_triangle(surf, T);
		imp_object_delete(T);
	}

	list_remove_all_data(to_remove);
	list_delete(to_remove);
}

static ImpTriangle * __get_triangle_with_same_color(ImpTriangle * T, ImpMatrix * BW) {

	ImpTriangle * t = NULL;
	ImpPoint p;
	int i;

	real color_T, color_t;

	p = imp_triangle_barycenter(T);
	color_T = BW->data[ imp_matrix_idx(BW, p.x, p.y) ];

	for (i = 0; i < 3; i++) {
		t = imp_triangle_get_neighbor(T, T->e[i]);

		if (!t) continue;

		p = imp_triangle_barycenter(t);
		color_t = BW->data[ imp_matrix_idx(BW, p.x, p.y) ];

		if (color_T == color_t) {
			return t;
		}
	}
	return NULL;
}

static int __count_triangle_neighbors_with_diferent_color(ImpTriangle * T, ImpMatrix * BW) {

	ImpTriangle * t = NULL;
	ImpPoint p;
	int i;

	real color_T, color_t;
	int nd;

	p = imp_triangle_barycenter(T);
	color_T = BW->data[ imp_matrix_idx(BW, p.x, p.y) ];


	nd = 0;
	for (i = 0; i < 3; i++) {
		t = imp_triangle_get_neighbor(T, T->e[i]);

		if (!t) continue;

		p = imp_triangle_barycenter(t);
		color_t = BW->data[ imp_matrix_idx(BW, p.x, p.y) ];

		if (color_T != color_t) nd++;
	}
	return nd;
}

/**
 * \brief Removes all triangles with the number of different color neighbors less than 2 or greater than 4.
 * @param surf
 * @param BW
 * @param changes
 */
static void __removes_triangles_by_different_color_neighbors_count(ImpSurface * surf, ImpMatrix * BW, int * changes) {
	ListIterator * it = NULL;

	List * to_remove = list_new();

	(*changes) = 0;

	it = list_get_first(surf->faces);
	while (it) {

		ImpTriangle * T;
		ImpTriangle * t;

		int ndT, ndt, nd;

		T = IMP_TRIANGLE(it->data);
		it = it->next;

		t = __get_triangle_with_same_color(T, BW);

		if (!t) {
			list_append(to_remove, T);
			(*changes)++;
			continue;
		}

		ndT = __count_triangle_neighbors_with_diferent_color(T, BW);
		ndt = __count_triangle_neighbors_with_diferent_color(t, BW);

		nd = ndT + ndt;

		if (nd < 2 || nd > 4) {
			list_append(to_remove, T);
			(*changes)++;
			continue;
		}
	}

	it = list_get_first(to_remove);
	while (it) {

		ImpTriangle * T;
		T = IMP_TRIANGLE(it->data);
		it = it->next;

		imp_surface_remove_triangle(surf, T);
		imp_object_delete(T);
	}

	list_remove_all_data(to_remove);
	list_delete(to_remove);
}

// </editor-fold>

void filt_chessboard(ImpSurface * surf, ImpMatrix * BW) {

	int changes = 0;

	// = Removes invalid vertexes in the surface ===============================
	__remove_invalid_points(surf, &changes);

	// = Removes invalid triangles by area =====================================
	__removes_triangles_by_area(surf, 50, 90000, &changes);

	// = Removes all triangles with color transition inside ====================
	__removes_triangles_with_color_transition(surf, BW, 0.4, &changes);

	// = Removes all triangles with same color neighbor different of 1 =========
	do {
		__removes_triangles_by_same_color_neighbors_count(surf, BW, &changes);
	} while (changes);

	// = Removes all triangles with different colors neighbor less than 2 or greater than 8
	do {
		__removes_triangles_by_different_color_neighbors_count(surf, BW, &changes);
	} while (changes);

}

// <editor-fold defaultstate="collapsed" desc="Auxiliar Create Chessboard Correspondence">

static void __get_central_triangles(ImpSurface * surf, ImpMatrix * BW, ImpTriangle ** _T1, ImpTriangle ** _T2, real * _d) {

	ImpTriangle * T1 = NULL;
	ImpTriangle * T2 = NULL;
	ImpPoint pi;
	ImpPoint pt;

	pi.x = BW->columns / 2;
	pi.y = BW->rows / 2;
	pi.z = 0.0;

	// = Get the triangle closest to image center ==============================
	ListIterator * it;
	real dt, d;
	it = list_get_first(surf->faces);

	// Initialize with big distance value.
	d = BW->columns + BW->rows;

	// Find the triangle.
	while (it) {

		pt = imp_triangle_barycenter(IMP_TRIANGLE(it->data));
		dt = imp_point_distance(&pt, &pi);

		if (dt < d) {
			d = dt;
			T1 = it->data;
		}

		it = it->next;
	}

	if (T1 == NULL) return;

	T2 = __get_triangle_with_same_color(T1, BW);

	if (T2 == NULL) return;

	*_T1 = T1;
	*_T2 = T2;
	*_d = d;
}

static real __triangle_color(ImpTriangle * T, ImpMatrix * BW) {
	ImpPoint p;
	p = imp_triangle_barycenter(T);
	return BW->data[ imp_matrix_idx(BW, p.x, p.y) ];
}

void __assing_grid(ImpMatrix * BW, ImpTriangle * T, ImpTriangle * Tant, ImpVertex * v, ImpVertex * vh, ImpVertex * vv, int * count) {

	_exit_if_fail(vh || vv);

	ImpEdge * eop = imp_triangle_get_opposite_edge(T, v);
	ImpTriangle * Top = imp_triangle_get_neighbor(T, eop);

	if (Top == NULL) {
		if (vh) {
			SWAP(v, vh, ImpVertex *);
		} else {
			if (vv) SWAP(v, vv, ImpVertex *);
		}
	} else {
		if (__triangle_color(T, BW) != __triangle_color(Top, BW)) {
			if (vh) {
				SWAP(v, vh, ImpVertex *);
			} else {
				if (vv) SWAP(v, vv, ImpVertex *);
			}
		}
	}

	eop = imp_triangle_get_opposite_edge(T, v);
	Top = imp_triangle_get_neighbor(T, eop);

	if (Top == NULL) {
		return;
	}

	ImpVertex * vop = imp_triangle_get_opposite_vertex(Top, eop);

	if (vop == NULL) {
		return;
	}

	// = Initialization ========================================================
	if (Tant == NULL) {

		GRID(v)->x = 0;
		GRID(v)->y = 0;

		GRID(vh)->x = GRID(v)->x + 1;
		GRID(vh)->y = GRID(v)->y;

		GRID(vv)->x = GRID(v)->x;
		GRID(vv)->y = GRID(v)->y + 1;

		GRID(vop)->x = GRID(vh)->x;
		GRID(vop)->y = GRID(vv)->y;

		/*
				grid[ v->id ].gx = 0;
				grid[ v->id ].gy = 0;
				grid[ vh->id ].gx = grid[ v->id ].gx + 1;
				grid[ vh->id ].gy = grid[ v->id ].gy;
				grid[ vv->id ].gx = grid[ v->id ].gx;
				grid[ vv->id ].gy = grid[ v->id ].gy + 1;
				grid[ vop->id ].gx = grid[ vh->id ].gx;
				grid[ vop->id ].gy = grid[ vv->id ].gy;
		 */
	}

	// =  ======================================================================
	ImpEdge * ex = NULL;
	ImpEdge * ey = NULL;

	ImpVertex * vaux = NULL;

	// = Computes vh  ==========================================================
	if (vh == NULL) {
		ey = imp_triangle_get_edge_that_connects_vertices(T, v, vv);
		vh = imp_triangle_get_opposite_vertex(T, ey);

		vaux = imp_triangle_get_opposite_vertex(Tant, ey);

		GRID(vh)->x = 2 * GRID(v)->x - GRID(vaux)->x;
		GRID(vh)->y = GRID(v)->y;

		GRID(vop)->x = GRID(vh)->x;
		GRID(vop)->y = GRID(vv)->y;

		/*
				grid[ vh->id ].gx = 2 * grid[ v->id ].gx - grid[ vaux->id ].gx;
				grid[ vh->id ].gy = grid[ v->id ].gy;
				grid[ vop->id ].gx = grid[ vh->id ].gx;
				grid[ vop->id ].gy = grid[ vv->id ].gy;
		 */
	}

	// = Computes vv ===========================================================
	if (vv == NULL) {
		ex = imp_triangle_get_edge_that_connects_vertices(T, v, vh);
		vv = imp_triangle_get_opposite_vertex(T, ex);

		vaux = imp_triangle_get_opposite_vertex(Tant, ex);

		GRID(vv)->x = GRID(v)->x;
		GRID(vv)->y = 2 * GRID(v)->y - GRID(vaux)->y;

		GRID(vop)->x = GRID(vh)->x;
		GRID(vop)->y = GRID(vv)->y;

		/*
				grid[ vv->id ].gx = grid[ v->id ].gx;
				grid[ vv->id ].gy = 2 * grid[ v->id ].gy - grid[ vaux->id ].gy;
				grid[ vop->id ].gx = grid[ vh->id ].gx;
				grid[ vop->id ].gy = grid[ vv->id ].gy;
		 */

	}

	int K;

	K = GRID(v)->x;
	K = GRID(v)->y;
	K = GRID(v)->visited;

	K = GRID(vv)->x;
	K = GRID(vv)->y;
	K = GRID(vv)->visited;

	K = GRID(vh)->x;
	K = GRID(vh)->y;
	K = GRID(vh)->visited;

	if (!GRID(v)->visited) {
		(*count)++;
		GRID(v)->visited = TRUE;
	}
	if (!GRID(vh)->visited) {
		(*count)++;
		GRID(vh)->visited = TRUE;
	}
	if (!GRID(vv)->visited) {
		(*count)++;
		GRID(vv)->visited = TRUE;
	}
	if (!GRID(vop)->visited) {
		(*count)++;
		GRID(vop)->visited = TRUE;
	}

	// =  ======================================================================
	if (!ex) ex = imp_triangle_get_edge_that_connects_vertices(T, v, vh);
	if (!ey) ey = imp_triangle_get_edge_that_connects_vertices(T, v, vv);

	// =  ======================================================================

	T_OPT(T)->visited = TRUE;
	T_OPT(Top)->visited = TRUE;

	// =  ======================================================================
	ImpTriangle * Tx = imp_triangle_get_neighbor(T, ex);
	ImpTriangle * Ty = imp_triangle_get_neighbor(T, ey);

	ImpEdge * e = NULL;

	e = imp_triangle_get_edge_that_connects_vertices(Top, vop, vv);
	ImpTriangle * Topx = imp_triangle_get_neighbor(Top, e);

	e = imp_triangle_get_edge_that_connects_vertices(Top, vh, vop);
	ImpTriangle * Topy = imp_triangle_get_neighbor(Top, e);

	if (Tx) if (!T_OPT(Tx)->visited) __assing_grid(BW, Tx, T, v, vh, NULL, count);
	if (Ty) if (!T_OPT(Ty)->visited) __assing_grid(BW, Ty, T, v, NULL, vv, count);

	if (Topx) if (!T_OPT(Topx)->visited) __assing_grid(BW, Topx, Top, vop, vv, NULL, count);
	if (Topy) if (!T_OPT(Topy)->visited) __assing_grid(BW, Topy, Top, vop, NULL, vh, count);

}

static int __compare_grid(void * a, void * b) {
	struct _chess_point * pa = (struct _chess_point *) a;
	struct _chess_point * pb = (struct _chess_point *) b;
	int r;

	r = pa->gy - pb->gy;
	if (r) return r;
	else return pa->gx - pb->gx;
}

// </editor-fold>

ChessGrid * create_chessboard_correspondence(ImpSurface * surf, ImpMatrix * BW) {

	ImpTriangle * T1 = NULL;
	ImpTriangle * T2 = NULL;

	ImpVertex * v = NULL;
	ImpEdge * es = NULL;

	real MAX_VAL = 999999;

	real d;

	if (surf->vertices->size == 0) return NULL;

	__get_central_triangles(surf, BW, &T1, &T2, &d);

	if (T1 == NULL || T2 == NULL) return NULL;

	// = Insert control structure to the triangles ============================

	ListIterator * it;
	it = list_get_first(surf->faces);
	while (it) {
		ImpTriangle * t;
		t = IMP_TRIANGLE(it->data);

		struct __triang_opt * topt;
		topt = mem_alloc(struct __triang_opt);
		topt->visited = FALSE;

		(IMP_OBJECT(it->data))->user_ptr = topt;

		it = it->next;
	}

	// = Insert control structure to the vertices ==============================
	it = list_get_first(surf->vertices);
	while (it) {

		v = IMP_VERTEX(it->data);
		struct __grid_point * grid;
		grid = mem_alloc(struct __grid_point);
		grid->visited = FALSE;
		grid->x = MAX_VAL; // only for post-verification
		grid->y = MAX_VAL;

		IMP_OBJECT(v)->user_ptr = grid;

		struct __grid_point * grid2;
		grid2 = IMP_OBJECT(v)->user_ptr;

		it = it->next;
	}

	es = imp_triangle_get_shared_edge(T1, T2);
	v = imp_triangle_get_opposite_vertex(T1, es);

	int count = 0;
	__assing_grid(BW, T1, NULL, v, IMP_SEGMENT(es)->v[0], IMP_SEGMENT(es)->v[1], &count);


	if (count <= 0) {
		count = 1;
	}

	ChessGrid * cgrid = mem_alloc(ChessGrid);
	cgrid->size = count;
	cgrid->points = mem_allocn(count, struct _chess_point);

	count = 0;

	// = Extract coordinates correspondence ====================================
	it = list_get_first(surf->vertices);
	while (it) {

		ImpVertex * v;
		struct __grid_point * grid;

		v = IMP_VERTEX(it->data);
		it = it->next;
		grid = GRID(v);

		if (grid->visited) {
			cgrid->points[count].gx = grid->x;
			cgrid->points[count].gy = grid->y;
			cgrid->points[count].x = IMP_POINT(v)->x;
			cgrid->points[count].y = IMP_POINT(v)->y;
			count++;
		}

		mem_free(IMP_OBJECT(v)->user_ptr);
	}

	qsort(cgrid->points, count, sizeof (struct _chess_point), (__compar_fn_t) __compare_grid);

	// = Remove Triangle properties ============================================
	it = list_get_first(surf->faces);
	while (it) {
		ImpTriangle * t;
		t = IMP_TRIANGLE(it->data);
		it = it->next;
		mem_free(IMP_OBJECT(t)->user_ptr);
	}

	return cgrid;

	/*
		int c;
		for (c = 0; c < surf->vertices->size; c++) {
			if (cgrid->points[ c ].gx == MAX_VAL && cgrid->points[ c ].gy == MAX_VAL) {
				break;
			}
		}

		cgrid->size = c;
		if (c != surf->vertices->size) {
			cgrid->points = (struct _chess_point *) realloc(cgrid->points, c * sizeof (struct _chess_point));
		}

		return cgrid;
	 */
}

void extract_matrices_from_chessgrid(ChessGrid * cgrid, ImpMatrix ** Xw, ImpMatrix ** Xi) {

	// = Convert grid to Matrix ================================================

	*Xw = imp_matrix_new(2, cgrid->size);
	*Xi = imp_matrix_new(2, cgrid->size);

	int d = cgrid->size;
	int i;
	for (i = 0; i < cgrid->size; i++) {

		(*Xw)->data[i + 0] = cgrid->points[i].gx;
		(*Xw)->data[i + d] = cgrid->points[i].gy;

		(*Xi)->data[i + 0] = cgrid->points[i].x;
		(*Xi)->data[i + d] = cgrid->points[i].y;

	}

}

inline real __mean_runtime(real val, int id, real last_mean) {
	return last_mean + (val - last_mean) / id;
}

void update_points_to_sub_pixel(ImpMatrix * Mat, ImpMatrix * X) {

	int w = 3;
	int i;
	int k;

	ImpMatrix * Mask = NULL;
	ImpMatrix * Dx = NULL;
	ImpMatrix * Dy = NULL;
	ImpMatrix * Dxy = NULL;
	ImpMatrix * Dxx = NULL;
	ImpMatrix * Dyy = NULL;
	ImpMatrix * Hresp = NULL;
	ImpMatrix * NMS = NULL;

	// = Create mask ===========================================================
	Mask = imp_matrix_new(Mat->rows, Mat->columns);

	for (k = 0; k < X->columns; k++) {
		int r = X->data[k + X->columns];
		int c = X->data[k];
		r = r - w;
		c = c - w;
		__mset(2 * w + 1, 2 * w + 1, &Mask->data[r * Mask->columns + c], Mask->columns, 1.0);
	}

	//imp_image_save_matrix_as_image(Mask, TRUE, "img/03-mask.pgm");

	// = Computes Dx, Dy, Dxy, ...
	imp_matrix_derivative_with_mask(Mat, Mask, &Dx, &Dy, &Dxy);
	imp_matrix_derivative2_with_mask(Mat, Mask, &Dxx, &Dyy);
	//imp_matrix_derivative(Mat, &Dx, &Dy, &Dxy);
	//imp_matrix_derivative2(Mat, &Dxx, &Dyy);


	// = Computes local x-corner response from Hessian =========================
	Hresp = imp_matrix_new(Mat->rows, Mat->columns);
	for (i = 0; i < Mat->size; i++) {

		if (Mask->data[i] == 0) continue;

		real rxx = Dxx->data[i];
		real ryy = Dyy->data[i];
		real rxy = Dxy->data[i];

		// = The corner is the local negative extremum S.
		real S = (rxx * ryy) - (rxy * rxy);
		Hresp->data[i] = S;
	}

	// = Preparing data to non-maximal suppression
	for (i = 0; i < Mat->size; i++) {
		Hresp->data[i] = Hresp->data[i] *(-1);
	}

	// = Non-maximal suppression ===============================================
	imp_image_nonmaxsup_feature_nonzero(Hresp, w, &NMS);

	imp_image_threshold(NMS, 0, 0, 255, IMP_THRESHOLD_BINARY, &Hresp);

	// = Get centroid of maximal numbers =======================================
	for (k = 0; k < X->columns; k++) {

		int r = X->data[k + X->columns];
		int c = X->data[k];

		int r1 = r - w;
		int r2 = r + w;
		int c1 = c - w;
		int c2 = c + w;

		if (r1 < 0) r1 = 0;
		else if (r1 >= Mat->rows) r1 = Mat->rows - 1;
		if (c1 < 0) c1 = 0;
		else if (c1 >= Mat->columns) c1 = Mat->columns - 1;

		real r_mean = 0;
		real c_mean = 0;
		int id = 1;

		int ri, ci;
		for (ri = r1; ri <= r2; ri++) {
			for (ci = c1; ci <= c2; ci++) {

				i = ri * Mat->columns + ci;

				real val = NMS->data[i];

				if (val == 0) continue;

				real rx = Dx->data[i];
				real ry = Dy->data[i];
				real rxy = Dxy->data[i];
				real rxx = Dxx->data[i];
				real ryy = Dyy->data[i];

				// calculates sub-pixel
				real den = (rxx * ryy - rxy * rxy);
				real s = (ry * rxy - rx * ryy) / den;
				real t = (rx * rxy - ry * rxx) / den;

				r_mean = __mean_runtime(ri + t, id, r_mean);
				c_mean = __mean_runtime(ci + s, id, r_mean);

				if (id > 1) {
					int t = 0;
					k = t + k;
				}

				id++;
			}
		}

		// updating
		X->data[k + X->columns] = r_mean;
		X->data[k] = c_mean;

	}

	imp_object_delete(Mask);
	imp_object_delete(Dx);
	imp_object_delete(Dy);
	imp_object_delete(Dxy);
	imp_object_delete(Dxx);
	imp_object_delete(Dyy);
	imp_object_delete(Hresp);
	imp_object_delete(NMS);
}

boolean imp_image_chessboard_get_grid(ImpImage * iGray, ImpMatrix ** Xw, ImpMatrix ** Xi) {

	// =========================================================================
	ImpMatrix * Mat = NULL;
	ImpMatrix * MatConv = NULL;
	ImpMatrix * Corners = NULL;
	ImpMatrix * SubPixel = NULL;
	ImpMatrix * BW = NULL;
	ImpMatrix * NMS = NULL;

	// =========================================================================
	ImpSurface * surf = NULL;

	// =========================================================================
	imp_image_gray2mat(iGray, &Mat);

	int w = 3;
	ImpMatrix * Gauss = imp_filter_new_gaussian(w, w / 4.0);
	imp_dsp_conv(Mat, Gauss, &MatConv);

	imp_image_features_Xcorner(MatConv, &Corners);

	imp_object_delete(Mat);
	imp_object_delete(Gauss);

	imp_image_nonmaxsup_feature_nonzero(Corners, 3, &NMS);

	imp_image_threshold(NMS, 0, 0, 255, IMP_THRESHOLD_BINARY, NULL);

	surf = imp_delaunay_from_matrix(NMS, TRUE);

	if (surf == NULL) {

		imp_object_delete(MatConv);
		imp_object_delete(Corners);
		imp_object_delete(NMS);

		return FALSE;
	}

	if (surf->vertices->size == 0) {

		imp_object_delete(MatConv);
		imp_object_delete(Corners);
		imp_object_delete(NMS);

		imp_surface_remove_all_elements_full(surf);
		imp_object_delete(surf);

		return FALSE;
	}

	imp_image_threshold_adaptive(MatConv, IMP_THRESHOLD_ADAPTIVE_WELLNER_EXTENDED, &BW);

	filt_chessboard(surf, BW);

	if (surf->faces->size == 0) {

		imp_object_delete(MatConv);
		imp_object_delete(Corners);
		imp_object_delete(NMS);
		imp_object_delete(BW);

		imp_surface_remove_all_elements_full(surf);
		imp_object_delete(surf);

		return FALSE;
	}

	// = Clean =================================================================
	imp_surface_remove_orfans_edges(surf);
	imp_surface_remove_orfans_vertices(surf);

	ChessGrid * cgrid = NULL;
	cgrid = create_chessboard_correspondence(surf, BW);

	if (cgrid == NULL) {
		imp_object_delete(MatConv);
		imp_object_delete(Corners);
		imp_object_delete(SubPixel);

		imp_surface_remove_all_elements_full(surf);
		imp_object_delete(surf);

		imp_object_delete(BW);

		return FALSE;
	}

	// = Convert grid to Matrix ================================================

	*Xw = imp_matrix_new(2, cgrid->size);
	*Xi = imp_matrix_new(2, cgrid->size);

	int d = cgrid->size;
	int i;
	for (i = 0; i < cgrid->size; i++) {

		(*Xw)->data[i + 0] = cgrid->points[i].gx;
		(*Xw)->data[i + d] = cgrid->points[i].gy;

		(*Xi)->data[i + 0] = cgrid->points[i].x;
		(*Xi)->data[i + d] = cgrid->points[i].y;

	}

	// = Generate the mask to apply sub-pixel ==================================
	/*
		imp_matrix_clear(NMS);
		int k;
		for (k = 0; k < (*Xi)->columns; k++) {

			int r = (*Xi)->data[k + (*Xi)->columns];
			int c = (*Xi)->data[k];

			NMS->data[ r * NMS->columns + c ] = 1;
		}
	 */

	//imp_image_save_matrix_as_image(NMS, TRUE, "04 - NMS-filt.pgm");

	//imp_matrix_print(*Xi);

	update_points_to_sub_pixel(MatConv, *Xi);
	//__update_subpixel_in_region(MatConv, Xi, 3);
	/*
		imp_matrix_clear(BW);

		imp_image_morph_dilate_w(NMS, 3, &BW);

		imp_image_save_matrix_as_image(BW, TRUE, "05 - Dilate-filt.pgm");


		{
			ImpMatrix * Dx = NULL;
			ImpMatrix * Dy = NULL;
			ImpMatrix * Dxy = NULL;

			ImpMatrix * Dxx = NULL;
			ImpMatrix * Dyy = NULL;

			imp_matrix_derivative_with_mask(MatConv, BW, &Dx, &Dy, &Dxy);
			imp_matrix_derivative2_with_mask(MatConv, BW, &Dxx, &Dyy);

			int k;
			for (k = 0; k < (*Xi)->columns; k++) {

				int r = (*Xi)->data[k];

			}


			imp_object_delete(Dx);
			imp_object_delete(Dy);
			imp_object_delete(Dxy);
			imp_object_delete(Dxx);
			imp_object_delete(Dyy);
		}
	 */

	//__update_subpixel_in_region(MatConv, Xi, 5);

	//imp_algebra_max_row( *Xw, &Max, &MaxId);
	//imp_image_get_projected_grid(H, &Xpr, &Xpi);


	imp_object_delete(NMS);
	imp_object_delete(MatConv);
	imp_object_delete(Corners);
	imp_object_delete(SubPixel);
	imp_surface_remove_all_elements_full(surf);
	imp_object_delete(surf);
	imp_object_delete(BW);

	mem_free(cgrid->points);
	mem_free(cgrid);


	return TRUE;
}

void imp_image_chessboard_draw_grid(ImpImage * iRGB, ImpMatrix * Xw, ImpMatrix * Xi, boolean draw_line) {

	ImpDraw draw;
	int i, c;
	int x1, y1, x2, y2;

	draw = imp_draw_create(iRGB);

	x1 = Xi->data[0];
	y1 = Xi->data[Xi->columns];

	draw.fg = imp_color(255, 0, 0);
	imp_draw_circle(&draw, x1, y1, 3);
	imp_draw_circle(&draw, x1, y1, 2);

	draw.fg = imp_color_rgb_palette(0, 1);

	c = 0;

	for (i = 1; i < Xi->columns; i++) {

		x2 = Xi->data[i];
		y2 = Xi->data[i + Xi->columns];

		draw.fg = imp_color(255, 255, 0);
		imp_draw_circle(&draw, x2, y2, 3);
		imp_draw_circle(&draw, x2, y2, 4);

		if (draw_line) {

			if (Xw->data[i + Xw->columns ] != Xw->data[i + Xw->columns - 1]) {
				draw.fg = imp_color_rgb_palette(c, 1);
				c++;
			} else {
				draw.fg = imp_color(255, 255, 0);
			}
			imp_draw_segment(&draw, x1, y1, x2, y2);
			//imp_draw_segment(&draw, x1+1, y1, x2+1, y2);
		}


		x1 = x2;
		y1 = y2;
	}

}

void imp_image_get_projected_grid(ImpMatrix * H, ImpMatrix ** Xw, ImpMatrix ** Xi) {

	if (*Xw == NULL) (*Xw) = imp_matrix_new(2, 400);
	if (*Xi == NULL) (*Xi) = imp_matrix_new(2, 400);

	real * h = H->data;
	real xi, yi, zi;
	real xr, yr, zr = 1;

	int i = 0;
	for (yr = -10; yr < 10; yr++) {
		for (xr = -10; xr < 10; xr++) {

			(*Xw)->data[i] = yr;
			(*Xw)->data[i + 400] = xr;


			xi = h[0] * xr + h[1] * yr + h[2] * zr;
			yi = h[3] * xr + h[4] * yr + h[5] * zr;
			zi = h[6] * xr + h[7] * yr + h[8] * zr;

			(*Xi)->data[i] = xi / zi;
			(*Xi)->data[i + 400] = yi / zi;

			i++;
		}
	}
}

/*
typedef struct __grid_node {
	real ix, iy;
	real score;
} GridNode;
*/

ImpMatrix * get_search_grid(ImpMatrix * pXr, int width, int height, ImpMatrix * H) {

	int i;
	ImpMatrix * sXi = NULL; // search grid in image
	real * h = H->data;

	real minx, maxx;
	real miny, maxy;

	// = Get max and min at x and y coordinates ================================
	minx = maxx = pXr->data[0];
	miny = maxy = pXr->data[ pXr->columns ];
	for (i = 0; i < pXr->columns; i++) {

		if (pXr->data[i] < minx) minx = pXr->data[i];
		if (pXr->data[i] > maxx) maxx = pXr->data[i];

		if (pXr->data[i + pXr->columns] < miny) miny = pXr->data[i + pXr->columns];
		if (pXr->data[i + pXr->columns] > maxy) maxy = pXr->data[i + pXr->columns];
	}


	// = Calculates size of input rectangle
	int sx = (maxx - minx) + 1;
	int sy = (maxy - miny) + 1;

	if (width < height) {

		if (sy > width) {
			// ok
		} else if (sx > width) {
			// should invert coordinates
			//SWAP(sx, sy, int);
			SWAP(width, height, int);
		} else {
			// undetermined
			if (width > height) height = width;
			else width = height;
		}

	} else if (height < width) {

		if (sx > height) {
			// ok
		} else if (sy > height) {
			// should invert coordinates
			//SWAP(sx, sy, int);
			SWAP(width, height, int);
		} else {
			// undetermined
			if (width > height) height = width;
			else width = height;
		}

	} else {
		// width == height
	}

	// = Calculates size of grid search
	int gx = 2 * width - sx; // = (x-sx)*2 + sx;
	int gy = 2 * height - sy; // = (y-sy)*2 + sy;

	int sz = gx*gy;

	// = Get search grid in coordinate image space =============================
	sXi = imp_matrix_new(2, sz);

	int cx = (width - sx) - minx;
	int cy = (height - sy) - miny;

	int m, n;
	i = 0;
	for (m = 0; m < gy; m++) {
		for (n = 0; n < gx; n++) {

			real xr = n - cx;
			real yr = m - cy;
			real zr = 1;

			real xi = h[0] * xr + h[1] * yr + h[2] * zr;
			real yi = h[3] * xr + h[4] * yr + h[5] * zr;
			real zi = h[6] * xr + h[7] * yr + h[8] * zr;

			sXi->data[ i ] = xi / zi;
			sXi->data[ i + sz ] = yi / zi;

			i++;

		}
	}

	return sXi;
}


